#include <stdio.h>
#include <errno.h>
#include <string.h>

#include <wiringPi.h>
#include <softPwm.h>
#include "dc_motor.h"


static unsigned char latch_state;

//set pin to do PWM function
int motorMap [] = {0,MOTOR_1_PWM, MOTOR_2_PWM,MOTOR_3_PWM,MOTOR_4_PWM};
struct _motorInf_t motorInf[DC_MOTORS];

void latch_tx(void)
{
   unsigned char i;

   digitalWrite(MOTORLATCH, LOW);

   digitalWrite(MOTORDATA, LOW);

   for (i=0; i<8; i++)
   {
      digitalWrite(MOTORCLK, LOW);

      if (latch_state & _BV(7-i))
      {
         digitalWrite(MOTORDATA, HIGH);
      }
      else
      {
         digitalWrite(MOTORDATA, LOW);
      }
      digitalWrite(MOTORCLK, HIGH);
   }
   digitalWrite(MOTORLATCH, HIGH);
}


void enable(void)
{
   pinMode(MOTORLATCH,  OUTPUT);
   pinMode(MOTORENABLE, OUTPUT);
   pinMode(MOTORDATA,   OUTPUT);
   pinMode(MOTORCLK,    OUTPUT);

   latch_state = 0;

   latch_tx();

   digitalWrite(MOTORENABLE, LOW); // active low
}



void DCMotorInit()
{
	
	  uint8_t  freq=FREQ_MAX;   //default value ; range : 0(min)~100(max)
  
   int i;
   enable();
   
   latch_state=0;
   latch_tx();
   for (i=0;i<DC_MOTORS;i++) {
   	  motorInf[i].pwmfreq = freq;  
      initPWM(i, freq);
      
   }   
#if 0
   switch (num)
   {
      case 1:
         latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B);
         latch_tx();
         initPWM(1, freq);
         break;
      case 2:
         latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B);
         latch_tx();
         initPWM(2, freq);
         break;
      case 3:
         latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B);
         latch_tx();
         initPWM(3, freq);
         break;
      case 4:
         latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B);
         latch_tx();
       initPWM(4, freq);
         break;
   }
   
#endif   
}



void initPWM(uint8_t num, int freq)
{
   softPwmCreate (motorMap[num], 0, RANGE) ;
   setPWM(num, freq);
}


void setPWM(uint8_t num, int freq)
{
   softPwmWrite (motorMap[num], freq) ;
}

void DCMotorRun(uint8_t motornum, uint8_t cmd)
{
   uint8_t a, b;

   switch (motornum)
   {
      case 1:
         a = MOTOR1_A; b = MOTOR1_B;
         break;
      case 2:
         a = MOTOR2_A; b = MOTOR2_B;
         break;
      case 3:
         a = MOTOR3_A; b = MOTOR3_B;
         break;
      case 4:
         a = MOTOR4_A; b = MOTOR4_B;
         break;
      default:
         return;
   }
  
   switch (cmd)
   {
      case FORWARD:
         latch_state |= _BV(a);
         latch_state &= ~_BV(b); 
         latch_tx();
         break;
      case BACKWARD:
         latch_state &= ~_BV(a);
         latch_state |= _BV(b); 
         latch_tx();
         break;
      case RELEASE:
         latch_state &= ~_BV(a);
         latch_state &= ~_BV(b); 
         latch_tx();
         initPWM(motornum, FREQ_MAX); 
       break;
   }
}

void DCMotorSetSpeed(uint8_t motornum, int speed)
{
   switch (motornum)
   {
      case 1:
         setPWM(1, speed);
         break;
      case 2:
         setPWM(2, speed);
         break;
      case 3:
         setPWM(3, speed);
         break;
      case 4:
         setPWM(4, speed);
         break;
   }
}